/**
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  * @file       rescue_task.h
  * @brief      救援任务，实施救援我方机器人和抓取障碍
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2021/12/20 16:01     yanfubing              1. done
  *
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  */
#ifndef RESCUE_TASK_H
#define RESCUE_TASK_H
#include "bsp_can.h"
#include "CAN_receive.h"
#include "remote_control.h"
#include "main.h"
#include "pid.h"
#include "user_lib.h"

#define INIT_CURRENT    1000
//电机码盘值最大以及中值
#define ECD_RANGE       8191
#define HALF_ECD_RANGE  4096
//电机的RMP转换为角度

#define MOTOR_ECD_TO_ANGLE     0.0023f
//救援障碍块任务延时
#define calibrate_time   500  //(ms)
#define rescue_task_INIT_TIME   5   //(ms)
//夹取任务一阶滤波系数
#define engineering_CONTROL_TIME 0.002f 
//救援电机最大速度（只按方向键）
#define obstacle_MAX_SPEED 1.0f
//障碍块最大速度（只按方向键）
//#define fall_down_MAX_SPEED 1.0f
//夹具旋转最大速度（只按方向键）
#define rescue_MAX_turn_SPEED 1.0f
/*************障碍块任务电机，速度闭环PID M3508*********/
#define M3508_obstacle_ANGLE_PID_KP 50
#define M3508_obstacle_ANGLE_PID_KI 0.0
#define M3508_obstacle_ANGLE_PID_KD 0.1
#define M3508_obstacle_ANGLE_PID_MAX_OUT 3500
#define M3508_obstacle_ANGLE_PID_MAX_IOUT 10

#define M3508_obstacle_SPEED_PID_KP 10
#define M3508_obstacle_SPEED_PID_KI 0.0
#define M3508_obstacle_SPEED_PID_KD 0.8
#define M3508_obstacle_SPEED_PID_MAX_OUT 2500
#define M3508_obstacle_SPEED_PID_MAX_IOUT 10
/**************救援任务电机，速度闭环PID M2006**********/

#define M2006_rescue_ANGLE_PID_KP 50     //速度PID
#define M2006_rescue_ANGLE_PID_KI 0
#define M2006_rescue_ANGLE_PID_KD 0.1
#define M2006_rescue_ANGLE_PID_MAX_OUT 4000
#define M2006_rescue_ANGLE_PID_MAX_IOUT 80

#define M2006_rescue_SPEED_PID_KP 15       //速度PID
#define M2006_rescue_SPEED_PID_KI 0
#define M2006_rescue_SPEED_PID_KD 0.3
#define M2006_rescue_SPEED_PID_MAX_OUT 4000.0f
#define M2006_rescue_SPEED_PID_MAX_IOUT 80
/*******************滤波******************/
#define engineering_friction_NUM 0.1666666667f
#define engineering_translation_NUM 0.3333333333f
#define engineering_Clamp_turn_NUM 0.1666666667f
/*********救援，障碍块状态机********/

typedef enum
{
  RESCUE=0,    //救援
	OBSTACLE,    //障碍块
  EXCHANGRE,   //兑换
}
engineer_cahssis_work_mode_e;
typedef enum          
{
  //救援状态机初始化
   
	//救援状态机回收装备
	REST=0,    
  INIT=1,  	
} rescue_state_e;


typedef struct
{  
  const motor_measure_t* rescue_obstacle_motor_measure;
		int16_t  target_speed;//目标速度
		float target_angle;//目标角度
		float angle;//累积角度
		int16_t give_current;
} rescue_motor_t;


typedef struct 
{
    const RC_ctrl_t *rescue_RC;               //遥控器的指针
    const can2_receive_t *rescue_can_rc;     //can2接收的数据
    float obstacle_init_angle[2];
    float rescue_init_angle[2];
		rescue_state_e rescue_state;            //救援状态机
		rescue_state_e obstacle_state;         //障碍状态机
		engineer_cahssis_work_mode_e engineer_chassis_work_mode;    //底盘工程的工作模式
		rescue_motor_t engineering_rescue_motor_data[2];           //救援任务m2006电机控制数据
		rescue_motor_t engineering_obstacle_motor_data[2];        //障碍块m3508电机控制数据
		pid_type_def engineering_rescue_angle_pid[2];
		pid_type_def engineering_rescue_motor_pid[2];
		pid_type_def engineering_obstacle_angle_pid[2];
		pid_type_def engineering_obstacle_motor_pid[2];
						//使用一阶低通滤波减缓设定值
		first_order_filter_type_t engineering_cmd_slow_set_rescue; 
				//使用一阶低通滤波减缓设定值
		first_order_filter_type_t engineering_cmd_slow_set_obstacle; 

		fp32 rescue_motor_max_speed;               //救援电机最大速度
		fp32 obstacle_motor_max_speed;            //障碍块电机最大速度
	 
}rescue_control_t;

/**
	* @brief          救援任务
	* @param[in]      pvParameters: 空
	* @retval         none
	*/

extern void rescue_obstacle_task(void const* pvParameters); 

#endif
